//pub_eg.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/NavSatFix.h"
#include <sstream>
#include <string>
ros::Time TimestampToRosTime(std::string timestamp)
{
    size_t len = timestamp.length();
    size_t secLen = len - 9;
    std::string sec_string = timestamp.substr(0,secLen);
    std::string nsec_string = timestamp.substr(secLen,9);
    //while(nsec_string.length() < 9){
        //nsec_string += "0";
    //}
    return ros::Time(std::stoi(sec_string),std::stoi(nsec_string));
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "publisher");
    ros::NodeHandle n("~");
    ros::Publisher imuPub = n.advertise<sensor_msgs::Imu>("imu_data", 20);
    ros::Publisher gpsPub = n.advertise<sensor_msgs::NavSatFix>("gps_data", 20);
    ros::Rate loop_rate(10);
    std::cout<<"publish"<<std::endl;
    while(ros::ok())
    {
        
        sensor_msgs::Imu imuData;
        imuData.header.stamp =ros::Time().now();
        imuData.angular_velocity.x = 0.0003;
        imuData.angular_velocity.y = 0.6599;
        imuData.angular_velocity.z = 1.5846;


        sensor_msgs::NavSatFix gpsdata;
        gpsdata.header.stamp = ros::Time().now();
        gpsdata.altitude = 36.110156832;
        gpsdata.longitude = 117.11015632;
        gpsdata.latitude = 0.000001;
        
        imuPub.publish(imuData);
        gpsPub.publish(gpsdata);
        //chatter_pub.publish(imuData);
        ros::spinOnce();
        loop_rate.sleep();
        std::cout<<"imu loop"<<std::endl;
    }

    return 0;
}
